/*
 * Copyright (C) 2017, Jonathan Cacace

 * Email id : jonathan.cacace@gmail.com

 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.

*/


#ifndef TELEOP_PANEL_H
#define TELEOP_PANEL_H
#include <stdio.h>
#include <QPainter>
#include <QLineEdit>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QLabel>
#include <QTimer>
#include <geometry_msgs/Twist.h>
#include <QDebug>
#include <pluginlib/class_list_macros.h>

#ifndef Q_MOC_RUN 
	#include <ros/ros.h>
	#include <rviz/panel.h>
#endif

class QLineEdit;

namespace rviz_teleop_commander
{

// Here we declare our new subclass of rviz::Panel.  Every panel which
// can be added via the Panels/Add_New_Panel menu is a subclass of
// rviz::Panel.
// TeleopPanel will show a text-entry field to set the output topic
// and a 2D control area.  The 2D control area is implemented by the
// DriveWidget class, and is described there.

class TeleopPanel: public rviz::Panel
{
	// This class uses Qt slots and is a subclass of QObject, so it needs
	// the Q_OBJECT macro.
	Q_OBJECT
	public:
		// QWidget subclass constructors usually take a parent widget
		// parameter (which usually defaults to 0).  At the same time,
		// pluginlib::ClassLoader creates instances by calling the default
		// constructor (with no arguments).  Taking the parameter and giving
		// a default of 0 lets the default constructor work and also lets
		// someone using the class for something else to pass in a parent
		// widget as they normally would with Qt.
		TeleopPanel( QWidget* parent = 0 );

		// Now we declare overrides of rviz::Panel functions for saving and
		// loading data from the config file.  Here the data is the
		// topic name.
		virtual void load( const rviz::Config& config );
		virtual void save( rviz::Config config ) const;

	  // Next come a couple of public Qt slots.
	public Q_SLOTS:
		// In this example setTopic() does not get connected to any signal
		// (it is called directly), but it is easy to define it as a public
		// slot instead of a private function in case it would be useful to
		// some other user.
		void setTopic( const QString& topic );


	// Here we declare some internal slots.
	protected Q_SLOTS:
		// sendvel() publishes the current velocity values to a ROS
		// topic.  Internally this is connected to a timer which calls it 10
		// times per second.
		void sendVel();


		void update_Linear_Velocity();
		void update_Angular_Velocity();

		// updateTopic() reads the topic name from the QLineEdit and calls
		// setTopic() with the result.
		void updateTopic();

		// Then we finish up with protected member variables.
	protected:
		
		// One-line text editor for entering the outgoing ROS topic name.
		QLineEdit* output_topic_editor_;
		// The current name of the output topic.
		QString output_topic_;
		QLineEdit* output_topic_editor_1;
		// The current name of the output topic.
		QString output_topic_1;
		QLineEdit* output_topic_editor_2;
		// The current name of the output topic.
		QString output_topic_2;

		// The ROS publisher for the command velocity.
		ros::Publisher velocity_publisher_;
		// The ROS node handle.
		ros::NodeHandle nh_;
		// The latest velocity values from the drive widget.
		float linear_velocity_;
		float angular_velocity_;
	};
} // end namespace rviz_teleop_commander

#endif // TELEOP_PANEL_H

